#include "WPILib.h"
#include "NetworkTables/NetworkTable.h"
#include "Drive.h"
#include "CustomDS.h"
#include "ImageProcessor.h"

/*********************
 * Channel constants *
 *********************/
static const int ENCODER1_CHANNEL_1 = 2;
static const int ENCODER1_CHANNEL_2 = 3;
static const int ENCODER2_CHANNEL_1 = 4;
static const int ENCODER2_CHANNEL_2 = 5;
//static const char * CAMERA_IP = "10.47.86.11";
static const int LEFT_MOTOR_CHANNEL = 1;
static const int RIGHT_MOTOR_CHANNEL = 2;
static const int LEFT_JOYSTICK_CHANNEL = 1;
static const int RIGHT_JOYSTICK_CHANNEL = 2;
static const int COMPRESSOR_RELAY_CHANNEL = 1;
static const int COMPRESSOR_PRESSURESWITCH_CHANNEL = 1;


class Robot2013 : public IterativeRobot
{
/**********************/
/* Autonomous stuff   */
/**********************/
	typedef enum AutonomousState 
	{
		Begin, 
		LocateTargets, 
		DeterminePath,
		AutonomousDrive
	};
	double DistancesFromTargets [3];
	double AnglesToTargets [3];
	double DistancesToWall [3];
	bool StateCompleted;
	int Iter;
	double DistanceToWallFinal;
	double DistanceErrorMeasure;
	double DistanceToGo;
	double DistanceGone;
	double DistancePerEncoderUnit;
	AutonomousState Astate;
	Encoder enc;
	Encoder enc2;
/***************************
 * End of Autonomous stuff *
 ***************************/
	
	Drive * drive;
	CustomDS * DStation;
	//ImageProcessor * imageProcessing;
	Compressor * comp;
	Relay fan;
public:
	Robot2013(void):
		enc(ENCODER1_CHANNEL_1, ENCODER1_CHANNEL_2, true, Encoder::k4X),
		enc2(ENCODER2_CHANNEL_1, ENCODER2_CHANNEL_2, true, Encoder::k4X),
		fan(2)
	{
		drive = new Drive(LEFT_JOYSTICK_CHANNEL, RIGHT_JOYSTICK_CHANNEL, LEFT_MOTOR_CHANNEL, RIGHT_MOTOR_CHANNEL);
		DStation = new CustomDS(drive);
		//imageProcessing = new ImageProcessor(CAMERA_IP);
		comp = new Compressor(COMPRESSOR_PRESSURESWITCH_CHANNEL, COMPRESSOR_RELAY_CHANNEL);
		comp->Start();
		fan.Set(Relay::kForward);
		DStation->SetConfig();
	}
	void RobotInit()
	{
		fan.Set(Relay::kForward);
		drive->SetDriveSpeed(0.0, 0.0);
		DStation->SetConfig();
	}
	void DisabledInit()
	{
		drive->SetDriveSpeed(0.0, 0.0);
	}
	void AutonomousInit()
	{
		drive->SetDriveSpeed(0.0, 0.0);
		Astate = Begin;
	}
	void AutonomousPeriodic()
	{
		/*
		processImage();
		switch (Astate)
		{
		case Begin:
			Astate = LocateTargets;
			
		case LocateTargets:
			//locating targets code here, get distance and angle
			//DistanceFromTarget1 = Camera.GetDistance(1); // i don't know what it actually is/will be
			//AngleToTarget1 = Camera.GetAngle(1);
			//DistanceFromTarget2 = Camera.GetDistance(2);
			//AngleToTarget2 = Camera.GetAngle(2);
			StateCompleted = true;
			if (StateCompleted)
			{
				Astate = DeterminePath;
				StateCompleted = false;
			}
		case DeterminePath:
			for (Iter = 1; Iter <= 3; Iter++)
			{
				double tdist = DistancesFromTargets[Iter]; 
				double ang = AnglesToTargets[Iter];
				double wdist = tdist*sin(ang);
				DistancesToWall [Iter] = wdist;
			}
			DistanceToWallFinal = (DistancesToWall[1] + DistancesToWall[2] + DistancesToWall[3])/3;
			DistanceErrorMeasure = (fabs(DistancesToWall[1] - DistancesToWall[2]) + fabs(DistancesToWall[2] - DistancesToWall[2]) + fabs(DistancesToWall[1] - DistancesToWall[3]))/3;
			DistanceToGo = 324.88 - DistanceToWallFinal - DistanceErrorMeasure;
			StateCompleted = true;
			if (StateCompleted)
			{
				Astate = AutonomousDrive;
				StateCompleted = false;
			}
		case AutonomousDrive:
			DistanceGone = enc.GetDistance();
			if (DistanceGone <= (DistanceToGo - 24))
			{
				//myRobot.Drive(.75,0);
				SetMotors(.75, .75);
			}
			else if ((DistanceGone >= DistanceToGo - 24)&&(DistanceGone <= DistanceToGo))
			{
				//myRobot.Drive(.25,0);
				SetMotors(.25, .25);
			}
			else if (DistanceGone >= DistanceToGo)
			{
				//myRobot.Drive(0,0);
				SetMotors(0, 0);
			}	
		}*/
	}
	void TeleopInit()
	{
		printf("teleop\r\n");
		drive->SetDriveSpeed(0.0, 0.0);
		DStation->InitClimb();
		DStation->ResetLights();
	}
	void TeleopPeriodic()
	{
		drive->HandleDrive();
		DStation->HandleComponents();
		//imageProcessing->processImage();
	}
	~Robot2013()
	{
		delete comp;
		delete DStation;
		delete drive;
		//delete imageProcessing;
	}
};

START_ROBOT_CLASS(Robot2013);
